#ifndef __NUMERICS__
#define __NUMERICS__

#include <slu_ddefs.h>

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/vector_proxy.hpp>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_sparse.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/numeric/ublas/triangular.hpp>
#include <boost/numeric/ublas/storage.hpp>
#include <boost/tuple/tuple.hpp>

#include <vector>
#include <iostream>

#include "vector.h"

namespace math {

template<class T, class M>
boost::numeric::ublas::vector<T, M> BSA(const boost::numeric::ublas::matrix<T>& A, const boost::numeric::ublas::vector<T, M>& b) {
	return boost::numeric::ublas::solve(A, b, boost::numeric::ublas::upper_tag());
}

template<class T, class M>
boost::numeric::ublas::vector<T, M> FSA(const boost::numeric::ublas::matrix<T>& A, const boost::numeric::ublas::vector<T, M>& b) {
	return boost::numeric::ublas::solve(A, b, boost::numeric::ublas::lower_tag());
}

template<class T, class M>
void Householder(const boost::numeric::ublas::matrix<T>& A, std::vector< boost::numeric::ublas::vector<T, M> >& Q, boost::numeric::ublas::matrix<T>& R) {
	size_t n = A.size1();
	size_t m = A.size2();
	Q.resize(m);
	R = A;
	for(size_t k = 0; k < m; k++) {
		boost::numeric::ublas::vector<T, M> u = boost::numeric::ublas::zero_vector<T>(n - k);
		for (size_t i = k; i < n; i++)
			u(i-k) = R(i, k);
		T unorm = boost::numeric::ublas::norm_2(u);
		boost::numeric::ublas::vector<T, M> v;
		if(std::abs(u(0) - unorm) < 0.000000001) {
			u(0) = 1.0;
			v = u;
		} else {
			u(0) -= unorm;
			v = u / boost::numeric::ublas::norm_2(u);
		}

		for(size_t i = 0; i < m; i++) {
			T sum = 0.0;
			for(size_t j = k; j < n; j++)
				sum += R(j, i) * v(j-k);

			for(size_t j = k; j < n; j++)
				R(j, i) = R(j, i) - (T)2.0 * v(j-k) * sum;	
		}
		Q[k] = v;
	}

	if(n != m)
		R.resize(m, m);
}

template<class T, class M>
bool SolveHouseholder(const std::vector< boost::numeric::ublas::vector<T, M> >& Q, const boost::numeric::ublas::matrix<T>& R, const boost::numeric::ublas::vector<T, M>& b, boost::numeric::ublas::vector<T, M>& x) {
	size_t n = Q[0].size();
	size_t m = R.size2();
	boost::numeric::ublas::vector<T, M> z = b;

	for(size_t i = 0; i < m; i++) {
		const boost::numeric::ublas::vector<T, M>& v = Q[i];
		T sum = 0;
		for(size_t j = i; j < n; j++)
			sum += z(j) * v(j - i);
		
		boost::numeric::ublas::vector<T, M> q = (T)2.0 * sum * v;	
		for(size_t j = i; j < n; j++)
			z(j) -= q(j - i); 
	}

	if(n != m)	
		z.resize(m);

	x = BSA(R, z);

	return true;
}

template<class T, class M>
bool Solve(boost::numeric::ublas::matrix<T>& A, std::vector< boost::numeric::ublas::vector<T, M> >& b, std::vector< boost::numeric::ublas::vector<T, M> >& x) {
	if(b.size() != x.size())
		return false;
	
	std::vector< boost::numeric::ublas::vector<T, M> > Q;
	boost::numeric::ublas::matrix<T> R;
	Householder(A, Q, R);
	
	for(size_t i = 0; i < b.size(); i++) {
		if(!SolveHouseholder(Q, R, b[i], x[i]))
			return false;
	}

	return true;
}

template<class T, class M>
bool Solve(boost::numeric::ublas::matrix<T>& A, boost::numeric::ublas::vector<T, M>& b, boost::numeric::ublas::vector<T, M>& x) {
	std::vector< boost::numeric::ublas::vector<T, M> > Q;
	boost::numeric::ublas::matrix<T> R;
	Householder(A, Q, R);

	if(!SolveHouseholder(Q, R, b, x))
		return false;

	return true;
}

template<class T>
T Invert(boost::numeric::ublas::matrix<T>& A) {
	if(A.size1() == A.size2()) {
		if(A.size1() == 2) {
			T det = A(0, 0) * A(1, 1) - A(0, 1) * A(1, 0);
			
			// should not be zero
			assert(fabs(det) > 0.0000000000001);

			boost::numeric::ublas::matrix<T> Ainv(2, 2);
			Ainv(0, 0) = A(1, 1);
			Ainv(1, 0) = -A(1, 0);
			Ainv(0, 1) = -A(0, 1);
			Ainv(1, 1) = A(0, 0);

			T invDet = (T)1.0 / det;
			for (size_t i = 0; i < 2; i++)
				for (size_t j = 0; j < 2; j++)
					Ainv(i, j) *= invDet;

			A = Ainv;
			return det;
		} else if(A.size1() == 3) {
			T c0 = A(1, 1) * A(2, 2);
			T c1 = A(2, 1) * A(1, 2);
			T c2 = A(0, 1) * A(2, 2);
			T c3 = A(2, 1) * A(0, 2);
			T c4 = A(0, 1) * A(1, 2);
			T c5 = A(1, 1) * A(0, 2);
			T det = A(0, 0) * (c0 - c1) 
			      - A(1, 0) * (c2 - c3)
			      + A(2, 0) * (c4 - c5);

			// should not be zero
			assert(fabs(det) > 0.0000000000001);
	
			boost::numeric::ublas::matrix<T> Ainv(3, 3);
			Ainv(0, 0) = c0 - c1;
			Ainv(1, 0) = A(2, 0) * A(1, 2) - A(1, 0) * A(2, 2);
			Ainv(2, 0) = A(1, 0) * A(2, 1) - A(2, 0) * A(1, 1);
			Ainv(0, 1) = c3 - c2;
			Ainv(1, 1) = A(0, 0) * A(2, 2) - A(2, 0) * A(0, 2);
			Ainv(2, 1) = A(2, 0) * A(0, 1) - A(0, 0) * A(2, 1);
			Ainv(0, 2) = c4 - c5;
			Ainv(1, 2) = A(1, 0) * A(0, 2) - A(0, 0) * A(1, 2);
			Ainv(2, 2) = A(0, 0) * A(1, 1) - A(1, 0) * A(0, 1);

			T invDet = (T)(1.0 / det);
			for (size_t i = 0; i < 3; i++)
				for (size_t j = 0; j < 3; j++)
					Ainv(i, j) *= invDet;

			A = Ainv;
			return det;
		} else if(A.size1() == 4) {
			T det = (T)1 / (A(3, 0) * A(2, 1) * A(1, 2) * A(0, 3) - A(2, 0) * A(3, 1) * A(1, 2) * A(0, 3) - A(3, 0) * A(1, 1) * A(2, 2) * A(0, 3) + A(1, 0) * A(3, 1) * A(2, 2) * A(0, 3) + A(2, 0) * A(1, 1) * A(3, 2) * A(0, 3) - A(1, 0) * A(2, 1) * A(3, 2) * A(0, 3) - A(3, 0) * A(2, 1) * A(0, 2) * A(1, 3) + A(2, 0) * A(3, 1) * A(0, 2) * A(1, 3) + A(3, 0) * A(0, 1) * A(2, 2) * A(1, 3) - A(0, 0) * A(3, 1) * A(2, 2) * A(1, 3) - A(2, 0) * A(0, 1) * A(3, 2) * A(1, 3) + A(0, 0) * A(2, 1) * A(3, 2) * A(1, 3) + A(3, 0) * A(1, 1) * A(0, 2) * A(2, 3) - A(1, 0) * A(3, 1) * A(0, 2) * A(2, 3) - A(3, 0) * A(0, 1) * A(1, 2) * A(2, 3) + A(0, 0) * A(3, 1) * A(1, 2) * A(2, 3) + A(1, 0) * A(0, 1) * A(3, 2) * A(2, 3) - A(0, 0) * A(1, 1) * A(3, 2) * A(2, 3) - A(2, 0) * A(1, 1) * A(0, 2) * A(3, 3) + A(1, 0) * A(2, 1) * A(0, 2) * A(3, 3) + A(2, 0) * A(0, 1) * A(1, 2) * A(3, 3) - A(0, 0) * A(2, 1) * A(1, 2) * A(3, 3) - A(1, 0) * A(0, 1) * A(2, 2) * A(3, 3) + A(0, 0) * A(1, 1) * A(2, 2) * A(3, 3));

			boost::numeric::ublas::matrix<T>Ainv(4, 4);
			Ainv(0, 0) = (-A(3, 1) * A(2, 2) * A(1, 3) + A(2, 1) * A(3, 2) * A(1, 3) + A(3, 1) * A(1, 2) * A(2, 3) - A(1, 1) * A(3, 2) * A(2, 3) - A(2, 1) * A(1, 2) * A(3, 3) + A(1, 1) * A(2, 2) * A(3, 3)) * det;
			Ainv(0, 1) = (A(3, 1) * A(2, 2) * A(0, 3) - A(2, 1) * A(3, 2) * A(0, 3) - A(3, 1) * A(0, 2) * A(2, 3) + A(0, 1) * A(3, 2) * A(2, 3) + A(2, 1) * A(0, 2) * A(3, 3) - A(0, 1) * A(2, 2) * A(3, 3)) * det;
			Ainv(0, 2) = (-A(3, 1) * A(1, 2) * A(0, 3) + A(1, 1) * A(3, 2) * A(0, 3) + A(3, 1) * A(0, 2) * A(1, 3) - A(0, 1) * A(3, 2) * A(1, 3) - A(1, 1) * A(0, 2) * A(3, 3) + A(0, 1) * A(1, 2) * A(3, 3)) * det;
			Ainv(0, 3) = (A(2, 1) * A(1, 2) * A(0, 3) - A(1, 1) * A(2, 2) * A(0, 3) - A(2, 1) * A(0, 2) * A(1, 3) + A(0, 1) * A(2, 2) * A(1, 3) + A(1, 1) * A(0, 2) * A(2, 3) - A(0, 1) * A(1, 2) * A(2, 3)) * det;

			Ainv(1, 0) = (A(3, 0) * A(2, 2) * A(1, 3) - A(2, 0) * A(3, 2) * A(1, 3) - A(3, 0) * A(1, 2) * A(2, 3) + A(1, 0) * A(3, 2) * A(2, 3) + A(2, 0) * A(1, 2) * A(3, 3) - A(1, 0) * A(2, 2) * A(3, 3)) * det;
			Ainv(1, 1) = (-A(3, 0) * A(2, 2) * A(0, 3) + A(2, 0) * A(3, 2) * A(0, 3) + A(3, 0) * A(0, 2) * A(2, 3) - A(0, 0) * A(3, 2) * A(2, 3) - A(2, 0) * A(0, 2) * A(3, 3) + A(0, 0) * A(2, 2) * A(3, 3)) * det;
			Ainv(1, 2) = (A(3, 0) * A(1, 2) * A(0, 3) - A(1, 0) * A(3, 2) * A(0, 3) - A(3, 0) * A(0, 2) * A(1, 3) + A(0, 0) * A(3, 2) * A(1, 3) + A(1, 0) * A(0, 2) * A(3, 3) - A(0, 0) * A(1, 2) * A(3, 3)) * det;
			Ainv(1, 3) = (-A(2, 0) * A(1, 2) * A(0, 3) + A(1, 0) * A(2, 2) * A(0, 3) + A(2, 0) * A(0, 2) * A(1, 3) - A(0, 0) * A(2, 2) * A(1, 3) - A(1, 0) * A(0, 2) * A(2, 3) + A(0, 0) * A(1, 2) * A(2, 3)) * det;

			Ainv(2, 0) = (-A(3, 0) * A(2, 1) * A(1, 3) + A(2, 0) * A(3, 1) * A(1, 3) + A(3, 0) * A(1, 1) * A(2, 3) - A(1, 0) * A(3, 1) * A(2, 3) - A(2, 0) * A(1, 1) * A(3, 3) + A(1, 0) * A(2, 1) * A(3, 3)) * det;
			Ainv(2, 1) = (A(3, 0) * A(2, 1) * A(0, 3) - A(2, 0) * A(3, 1) * A(0, 3) - A(3, 0) * A(0, 1) * A(2, 3) + A(0, 0) * A(3, 1) * A(2, 3) + A(2, 0) * A(0, 1) * A(3, 3) - A(0, 0) * A(2, 1) * A(3, 3)) * det;
			Ainv(2, 2) = (-A(3, 0) * A(1, 1) * A(0, 3) + A(1, 0) * A(3, 1) * A(0, 3) + A(3, 0) * A(0, 1) * A(1, 3) - A(0, 0) * A(3, 1) * A(1, 3) - A(1, 0) * A(0, 1) * A(3, 3) + A(0, 0) * A(1, 1) * A(3, 3)) * det;
			Ainv(2, 3) = (A(2, 0) * A(1, 1) * A(0, 3) - A(1, 0) * A(2, 1) * A(0, 3) - A(2, 0) * A(0, 1) * A(1, 3) + A(0, 0) * A(2, 1) * A(1, 3) + A(1, 0) * A(0, 1) * A(2, 3) - A(0, 0) * A(1, 1) * A(2, 3)) * det;

			Ainv(3, 0) = (A(3, 0) * A(2, 1) * A(1, 2) - A(2, 0) * A(3, 1) * A(1, 2) - A(3, 0) * A(1, 1) * A(2, 2) + A(1, 0) * A(3, 1) * A(2, 2) + A(2, 0) * A(1, 1) * A(3, 2) - A(1, 0) * A(2, 1) * A(3, 2)) * det;
			Ainv(3, 1) = (-A(3, 0) * A(2, 1) * A(0, 2) + A(2, 0) * A(3, 1) * A(0, 2) + A(3, 0) * A(0, 1) * A(2, 2) - A(0, 0) * A(3, 1) * A(2, 2) - A(2, 0) * A(0, 1) * A(3, 2) + A(0, 0) * A(2, 1) * A(3, 2)) * det;
			Ainv(3, 2) = (A(3, 0) * A(1, 1) * A(0, 2) - A(1, 0) * A(3, 1) * A(0, 2) - A(3, 0) * A(0, 1) * A(1, 2) + A(0, 0) * A(3, 1) * A(1, 2) + A(1, 0) * A(0, 1) * A(3, 2) - A(0, 0) * A(1, 1) * A(3, 2)) * det;
			Ainv(3, 3) = (-A(2, 0) * A(1, 1) * A(0, 2) + A(1, 0) * A(2, 1) * A(0, 2) + A(2, 0) * A(0, 1) * A(1, 2) - A(0, 0) * A(2, 1) * A(1, 2) - A(1, 0) * A(0, 1) * A(2, 2) + A(0, 0) * A(1, 1) * A(2, 2)) * det;

			A = Ainv;
			return det;
		} else {
			std::cerr << "error invert: matrix dimension has to be 2 or 3." << std::endl; 
		}
	}
	return 0.0;
}

template<class T>
T Det(const boost::numeric::ublas::matrix<T>& A) {
	size_t n1 = A.size1();
	size_t n2 = A.size2();
	if(n1 == n2) {
		if(n1 == 2) {
			return A(0, 0) * A(1, 1) - A(0, 1) * A(1, 0);
		} else if(n1 == 3) {
			T d0 = A(1, 1) * A(2, 2) - A(1, 2) * A(2, 1);
			T d1 = A(1, 0) * A(2, 2) - A(1, 2) * A(2, 0);
			T d2 = A(1, 0) * A(2, 1) - A(1, 1) * A(2, 0);
			return A(0, 0) * d0 - A(0, 1) * d1 + A(0, 2) * d2;
		} else if(n1 == 4) {
			return A(0, 1) * A(1, 3) * A(2, 2) * A(3, 0) - A(0, 1) * A(1, 2) * A(2, 3) * A(3, 0) - A(0, 0) * A(1, 3) * A(2, 2) * A(3, 1) + A(0, 0) * A(1, 2) * A(2, 3) * A(3, 1) - A(0, 1) * A(1, 3) * A(2, 0) * A(3, 2) + A(0, 0) * A(1, 3) * A(2, 1) * A(3, 2) + A(0, 1) * A(1, 0) * A(2, 3) * A(3, 2) - 
				   A(0, 0) * A(1, 1) * A(2, 3) * A(3, 2) + A(0, 3) * (A(1, 2) * A(2, 1) * A(3, 0) - A(1, 1) * A(2, 2) * A(3, 0) - A(1, 2) * A(2, 0) * A(3, 1) + A(1, 0) * A(2, 2) * A(3, 1) + A(1, 1) * A(2, 0) * A(3, 2) - A(1, 0) * A(2, 1) * A(3, 2)) + A(0, 1) * A(1, 2) * A(2, 0) * A(3, 3) - 
				   A(0, 0) * A(1, 2) * A(2, 1) * A(3, 3) - A(0, 1) * A(1, 0) * A(2, 2) * A(3, 3) + A(0, 0) * A(1, 1) * A(2, 2) * A(3, 3) + A(0, 2) * (-(A(1, 3) * A(2, 1) * A(3, 0)) + A(1, 1) * A(2, 3) * A(3, 0) + A(1, 3) * A(2, 0) * A(3, 1) - A(1, 0) * A(2, 3) * A(3, 1) - A(1, 1) * A(2, 0) * A(3, 3) + A(1, 0) * A(2, 1) * A(3, 3));
		} else {
			std::cerr << "error Det: only 2x2 matrices supported." << std::endl;
		}
	} else {
		std::cerr << "error Det: matrix is not a square matrix." << std::endl;
	}
	return (T)0.0;
}

template<class type>
bool SolveCG(const boost::numeric::ublas::mapped_matrix<type>& A, const boost::numeric::ublas::vector<type, std::vector<type> >& b, boost::numeric::ublas::vector<type, std::vector<type> >& x, type eps) {
	size_t n = b.size();
	
	boost::numeric::ublas::vector<type, std::vector<type> > r = b;// - prod(A, x);
	boost::numeric::ublas::vector<type, std::vector<type> > v = r;
	type c = inner_prod(r, r);
	
	boost::numeric::ublas::vector<type, std::vector<type> > z(n);
	type t, d;
	
	size_t i;
	for(i = 0; i < n; i++) {
		if(sqrt(inner_prod(v, v)) < eps) {
			std::cerr << "Error in ConjugateGradient: execution of function terminated." << std::endl;
			return false;
		}

		z = prod(A, v);
		t = c / inner_prod(v, z);
		x += t * v;
		r -= t * z;
		d = inner_prod(r, r);
		if(std::sqrt(d) < eps) {
			std::cout << "iterations: " << i << ", error: " << std::sqrt(d) << std::endl;
			return true;
		}
		v = r + (d / c) * v;
		c = d;
	}
	std::cout << "could not solve system." << std::endl;

	return false;
}

}

#endif //__NUMERICS__
